using System;
using System.Collections.Generic;
using System.Text;
using System.Threading;
using FTD2XX_NET;
using System.IO;

namespace LoopBack
{
    class usb_comm
    {


        static FTDI myFtdiDevice = new FTDI();

        static void init()
        {
            FTDI.FT_STATUS ftStatus = FTDI.FT_STATUS.FT_OK;
            ftStatus = myFtdiDevice.OpenByDescription("USB-Blaster");
            myFtdiDevice.SetLatency(2);
            myFtdiDevice.InTransferSize(16384*4);

            UInt32 numBytesWritten = 0;

            byte[] Init_CMD = new byte[5];
            Init_CMD[0] = 0x26;
            Init_CMD[1] = 0x27;
            Init_CMD[2] = 0x26;
            Init_CMD[3] = 0x81;
            Init_CMD[4] = 0x00;
            myFtdiDevice.Write(Init_CMD, 3, ref numBytesWritten);
            myFtdiDevice.Purge(FTDI.FT_PURGE.FT_PURGE_RX);

            for (int i = 0; i < 262144; i++)
            {
                sram[i] = 255;
                sram_new[i] = true;
            }
        }


        static void close()
        {
            byte[] Close_CMD = new byte[1];
            Close_CMD[0] = 0x1F;

            UInt32 numBytesWritten = 0;

            myFtdiDevice.Write(Close_CMD, 1, ref numBytesWritten);

            Thread.Sleep(10);
            myFtdiDevice.Close();
        }


        static private void send_array(byte[] to_send, int bytes_to_send)
        {
            send_bytes_todo += bytes_to_send;

            UInt32 numBytesWritten = 0;
            myFtdiDevice.Write(to_send, bytes_to_send, ref numBytesWritten);
            send_bytes += (int)numBytesWritten;
        }


        static byte[] send_queue = new byte[1116];
        static int queue_counter = 0;

        static void send_packet(byte[] payload, byte module_nr , int imidiate)
        {
            if (queue_counter < 1050)
            {
                send_queue[queue_counter] = (byte)(192 + 5 + payload.Length);
                send_queue[queue_counter + 1] = 254;
                send_queue[queue_counter + 2] = 0;
                send_queue[queue_counter + 3] = module_nr;
                send_queue[queue_counter + 4] = (byte)payload.Length;
                send_queue[queue_counter + 5] = 60;
                queue_counter += 6;
                for (int i = 0; i < payload.Length; i++)
                {
                    send_queue[queue_counter] = payload[i];
                    queue_counter++;
                }
            }

            if (queue_counter > 1050 || imidiate==1)
            {
                send_array(send_queue,queue_counter);
                queue_counter = 0;
            }
        }



        static int receive_counter = 0;
        static byte[] receive_queue = new byte[65536];
        static object semaphore = ""; 

        static void receive()
        {

            UInt32 numBytesWritten = 0;
            UInt32 numBytesToRead = 0;
            bool done=false;

            while (1 == 1)
            {
                myFtdiDevice.GetRxBytesAvailable(ref numBytesToRead);
                byte[] received = new byte[numBytesToRead];
                myFtdiDevice.Read(received, numBytesToRead, ref numBytesWritten);

                rec_bytes += (int)numBytesWritten;

                done = false;

                while (done == false)
                {
                    lock (semaphore)
                    {
                        if (receive_counter + numBytesToRead < 65530)
                        {
                            for (int i = 0; i < numBytesToRead; i++)
                            {
                                receive_queue[receive_counter] = received[i];
                                receive_counter++;
                            }
                            done = true;
                        }
                    }
                    //Thread.Sleep(1);
                }
            }
        }


        static int last_number = 255;
        static int bytes_rec = 0;
        static int packets_rec = 0;

        static int send_bytes_todo = 0;
        static int send_bytes = 0;
        static int rec_bytes = 0;

        static void get_packets()
        {
            int packet_size = 0;
            
            int last = 0;
            int copy_to = 0;

            while (1 == 1)
            {

                lock (semaphore)
                {
                    if (receive_counter > 5)
                    {
                        last = receive_counter - 6;
                        for (int j = 0; j < receive_counter - 5; j++)
                        {
                            if (receive_queue[j] == 254 && receive_queue[j + 4] == 60)
                            {
                                packet_size = receive_queue[j + 3];
                                if (j + 5 + packet_size < receive_counter)
                                {
                                    for (int m = j + 5; m < j + 5 + packet_size; m++)
                                    {
                                        //Console.WriteLine(receive_queue[m]);
                                    }

                                    if ((byte)(last_number + 1) != receive_queue[j + 1])
                                    {
                                        Console.WriteLine("#############");
                                        Console.WriteLine(j + " | " + receive_counter);
                                        Console.WriteLine(last_number + " | " + receive_queue[j + 1]);
                                        //Console.ReadLine();
                                    }
                                    else
                                    {

                                        while (sram_packetqueue_empty == false) { sram_handle.Set(); }

                                        lock (sram_semaphore)
                                        {
                                            for (int m = j + 5; m < j + 5 + packet_size; m++)
                                            {
                                                sram_packetqueue[m - j - 5] = receive_queue[m];
                                                //Console.WriteLine(receive_queue[m]);
                                            }
                                            //Console.ReadLine();
                                            sram_packetqueue_empty = false;
                                        }

                                        sram_handle.Set();
                                    }


                                    last_number = receive_queue[j + 1];
                                    j = j + 5 + packet_size;

                                    packets_rec++;
                                    bytes_rec += packet_size;

                                    if (packets_rec % 100 == 0)
                                        Console.WriteLine(bytes_rec + " bytes| " + packets_rec + " packets| ");

                                }
                                else
                                {
                                    last = j;
                                    break;
                                }
                            }
                        }
                    }

                    for (int j = 0; j < receive_counter - last; j++)
                    {
                        receive_queue[j] = receive_queue[last + j];
                        copy_to = j;
                    }
                    receive_counter = copy_to + 1;
                }
                //Thread.Sleep(1);
            }
        }


        static void write_sram(UInt16[] to_write)
        {
            byte[] sram_writer = new byte[35];

            int posi = 0;

            for (int a = 0; a < 4; a++)
            {
                sram_writer[0] = (byte)(128 + a);
                for (int b = 0; b < 256; b++)
                {
                    sram_writer[1] = (byte)b;
                    for (int c = 0; c < 16; c++)
                    {
                        sram_writer[2] = (byte)(c*16);
                        for (int d = 0; d < 32; d++)
                        {
                            sram_writer[3+d] = (byte)(to_write[posi] >> 8);
                            d++;
                            sram_writer[3+d] = (byte)((UInt16)(to_write[posi] << 8) >> 8);
                            posi++;
                        }
                        send_packet(sram_writer, 1, 0);
                    }
                }
                Console.WriteLine(a);
            }
            send_packet(sram_writer, 0, 1);
        }


        static UInt16[] sram = new UInt16[262144];
        static bool[] sram_new = new bool[262144];
        static byte[] sram_packetqueue = new byte[64];
        static bool sram_packetqueue_empty = true;
        static object sram_semaphore = "";
        static Thread sram_worker = new Thread(new ThreadStart(sram_uptodate));
        static EventWaitHandle sram_handle = new AutoResetEvent(false);


        static void sram_uptodate()
        {
            int amount_of_bytes=0;
            uint address = 0;

            while (1 == 1)
            {
                sram_handle.WaitOne();

                //lock (sram_semaphore)
                {

                    //if (sram_packetqueue_empty == false)
                    {
                        amount_of_bytes = sram_packetqueue[0] >> 3;

                        address = (uint)(sram_packetqueue[0] << 30);
                        address = (uint)(address >> 30);
                        address = (uint)(address << 16);
                        address += (uint)(sram_packetqueue[1] << 8);
                        address += (uint)(sram_packetqueue[2]);

                        //Console.WriteLine(address);

                        for (int i = 3; i < (amount_of_bytes*2)+3; i++)
                        {
                            sram[address + ((i-3)/2)] = (UInt16)(sram_packetqueue[i]<<8);
                            i++;
                            sram[address + ((i-4)/2)] += sram_packetqueue[i];
                            sram_new[address + ((i - 3) / 2)] = false;
                        }
                    }
                    
                    //Console.WriteLine("-----------");
                    sram_packetqueue_empty = true;
                }
            }
        }


        static void fetch_sram()
        {

            byte[] packet_read_sram = new byte[3];
            byte[] packet = new byte[58];
            for (int i = 0; i < 58; i++)
            {
                packet[i] = 0x00;
            }

            Console.WriteLine("Start");

            for (int i = 0; i < 262144; i++)
            {
                //sram_new[i] = true;
            }

            bool all_fetched = false;
            int nr = 0;
            
            while (all_fetched == false)
            {
                all_fetched = true;
                for (int i = 0; i < 262144; i=i+16)
                {
                    if (sram_new[i] == true)
                    {
                        packet_read_sram[0] = (byte)(i>>16);
                        packet_read_sram[0] += 128;
                        packet_read_sram[1] = (byte)((i & 65280)>>8);
                        packet_read_sram[2] = (byte)(i & 255);

                        send_packet(packet_read_sram, 2, 0);
                        send_packet(packet, 0, 0);
                        all_fetched = false;
                    }
                }
                send_packet(packet, 0, 1);
                Thread.Sleep(1);
            }

        }


        static void file_to_core(int core)
        {
            byte[] source = null;
            FileStream fs = new FileStream("led_test.hac", FileMode.Open, FileAccess.Read);
            BinaryReader br = new BinaryReader(fs);
            int filesize = (int)(new FileInfo("led_test.hac").Length);
            source = br.ReadBytes(filesize);

            byte[] send_file = new byte[35];
            int bytecounter = 3;
            int sourcecounter = 0;

            send_file[0] = 128;
            send_file[1] = 0;
            send_file[2] = 0;


            while (sourcecounter < filesize)
            {
                send_file[bytecounter] = source[sourcecounter];
                bytecounter++;
                sourcecounter++;


                if (bytecounter == 35)
                {
                    send_packet(send_file, 1, 1);
                    bytecounter = 3;
                    send_file[2] += 16;
                    //Console.ReadLine();
                }
            }

            byte[] send_file2 = new byte[bytecounter];
            if (bytecounter > 3)
            {
                for (int i = 0; i < bytecounter; i++)
                    send_file2[i] = send_file[i];

                send_file2[0] = (byte)((bytecounter - 3) * 8);
                Console.WriteLine(bytecounter);
                Console.WriteLine(send_file2[0]);
                Console.WriteLine(send_file2[2]);
                send_packet(send_file2, 1, 1);
            }

            // start core !
            byte[] start_cpu = new byte[1];
            start_cpu[0] = (byte)core;
            send_packet(start_cpu, 4, 1);

        }


        static void Main(string[] args)
        {

            init();

            Thread read_worker = new Thread(new ThreadStart(receive));
            read_worker.Priority = Thread.CurrentThread.Priority - 1;
            read_worker.Start();

            Thread frame_worker = new Thread(new ThreadStart(get_packets));
            frame_worker.Start();

            sram_worker.Start();

            byte[] to_receive = new byte[4096];

            byte[] packet = new byte[58];
            for (int i = 0; i < 58; i++)
            {
                //packet[i]=(byte)(i);
                packet[i] = 0x00;
            }


            UInt16[] send_to_sram = new UInt16[262144];
            for (int i = 0; i < 262144; i++)
            {
                send_to_sram[i] = 0x000F;
            }
           
            //write_sram(send_to_sram);


            byte[] packet_led = new byte[7];
            packet_led[0] = 16;
            packet_led[1] = 0;
            packet_led[2] = 127;
            packet_led[3] = 0x00;
            packet_led[4] = 0x01;
            packet_led[5] = 0x00;
            packet_led[6] = 0x02;

            for (int i = 0; i < 0; i++)
            {
                packet_led[2]+=2;
                send_packet(packet_led,1,1);
                //Console.ReadLine();
            }


            file_to_core(1);


            //fetch_sram();

            close();

            
            for (int i = 0; i < 524288; i++)
            {
                Console.Write("{0:X}", Convert.ToInt32(sram[i]));
                Console.WriteLine(" | " + sram[i] + "|" + sram_new[i] + " | " +i);

                if (i % 50 == 0)
                {
                    Console.WriteLine("------------");
                    Console.ReadLine();
                }
            }

            Console.ReadLine();

            read_worker.Abort();
            frame_worker.Abort();
            sram_worker.Abort();

            return;

        }
    }
}
